
// import oimo.collision.raycast.*;
// import oimo.common.MathUtil;
// import oimo.common.Transform;
// import oimo.common.Vec3;
// import oimo.m.Vec3;
// import oimo.m.M;

import { ConvexGeometry } from "./ConvexGeometry";
import { GeometryType } from "./GeometryType";
import { MathUtil } from "../../common/MathUtil";
import { M } from "../../common/M";
import { Aabb } from "./Aabb";
import { Transform } from "../../common/Transform";
import { Vec3 } from "../../common/Vec3";
import { RayCastHit } from "./RayCastHit";

/**
 * A sphere collision geometry.
 */

export class SphereGeometry extends ConvexGeometry {
	_radius: number;

	/**
	 * Creates a sphere collision geometry of radius `radius`.
	 */
	constructor(radius: number) {
		super(GeometryType._SPHERE);
		this._radius = radius;
		this._gjkMargin = this._radius;
		this._updateMass();
	}

	/**
	 * Returns the radius of the sphere.
	 */
	public getRadius(): number {
		return this._radius;
	}

	public _updateMass(): void {
		this._volume = 4 / 3 * MathUtil.PI * this._radius * this._radius * this._radius;
		M.mat3_diagonal(this._inertiaCoeff,
			2 / 5 * this._radius * this._radius,
			2 / 5 * this._radius * this._radius,
			2 / 5 * this._radius * this._radius
		);
	}

	public _computeAabb(aabb: Aabb, tf: Transform): void {
		var radVec: Vec3 = new Vec3(this._radius, this._radius, this._radius);
		M.vec3_sub(aabb._min, tf._position, radVec);
		M.vec3_add(aabb._max, tf._position, radVec);
	}

	public computeLocalSupportingVertex(dir: Vec3, out: Vec3): void {
		out.zero();
	}

	public _rayCastLocal(begin: Vec3, end: Vec3, hit: RayCastHit): boolean {
		var d: Vec3 = new Vec3();
		M.vec3_sub(d, end, begin);

		var a: number = M.vec3_dot(d, d);
		var b: number = M.vec3_dot(begin, d);
		var c: number = M.vec3_dot(begin, begin) - this._radius * this._radius;

		var D: number = b * b - a * c;
		if (D < 0) return false;

		var t: number = (-b - MathUtil.sqrt(D)) / a;
		if (t < 0 || t > 1) return false;

		M.vec3_addRhsScaledTo(hit.position, begin, d, t);
		hit.position.normalzeTo(hit.normal);
		hit.fraction = t;
		return true;
	}

}
